from launch import LaunchDescription
from launch.actions import DeclareLaunchArgument
from launch.substitutions import LaunchConfiguration
from launch_ros.actions import Node

def generate_launch_description():
    # 每个相机的参数
    cam0_params = {
        "video_port": "/dev/video1",
        "output_rate": 10,
        "pub_tf": False,
        "image_raw_topic": "/camera0/image_raw",
        "camera_info_topic": "/camera0/color/camera_info",
        "image_rect_topic": "/camera0/image_rect",
        "calibration_file": "ost.yaml"
    }

    cam1_params = {
        "video_port": "/dev/video3",
        "output_rate": 10,
        "pub_tf": False,
        "image_raw_topic": "/camera1/image_raw",
        "camera_info_topic": "/camera1/color/camera_info",
        "image_rect_topic": "/camera1/image_rect",
        "calibration_file": "ost.yaml"
    }

    cam2_params = {
        "video_port": "/dev/video5",
        "output_rate": 10,
        "pub_tf": False,
        "image_raw_topic": "/camera2/image_raw",
        "camera_info_topic": "/camera2/color/camera_info",
        "image_rect_topic": "/camera2/image_rect",
        "calibration_file": "ost.yaml"
    }

    # 启动三个相机节点
    node_cam0 = Node(
        package="capture_image",
        executable="capture_image_node",
        name="capture_image_0",
        output="screen",
        parameters=[cam0_params],
    )

    node_cam1 = Node(
        package="capture_image",
        executable="capture_image_node",
        name="capture_image_1",
        output="screen",
        parameters=[cam1_params],
    )

    node_cam2 = Node(
        package="capture_image",
        executable="capture_image_node",
        name="capture_image_2",
        output="screen",
        parameters=[cam2_params],
    )

    return LaunchDescription([
        # declare_rviz_arg,
        node_cam0,
        node_cam1,
        node_cam2
    ])
